﻿using System;
using System.Drawing;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;

// Our warp matrix looks like this one:
//
//  ! 1  -wz  tx !
//  ! wz  1   ty !
//  ! 0   0   1  !
//
namespace ImageAlignment
{
  class AuxFunction
  {
    public static void InitWarp(Matrix<float> W, float wz, float tx, float ty)
    {
      var array = new float[3, 3];
      array[0, 0] = array[1, 1] = array[2, 2] = 1;
      array[0, 1] = -wz;
      array[0, 2] = tx;
      array[1, 0] = wz;
      array[1, 2] = ty;
      array[2, 0] = array[2, 1] = 0;
      W.Data = array;
    }

    public static void WarpImage(Image<Gray, byte> srcFrame, Image<Gray, byte> dstFrame, Matrix<float> W)
    {
      var X = new Matrix<float>(3, 1);
      var Z = new Matrix<float>(3, 1);
      int x, y;
      
      for (x = 0; x < srcFrame.Width; x++)
      {
        for (y = 0; y < srcFrame.Height; y++)
        {
          X = CreateVector(x, y, 1);

          Z = W.Mul(X);

          int x2, y2;

          x2 = (int)Z[0, 0];
          y2 = (int)Z[1, 0];

          if (x2 >= 0 && x2 < dstFrame.Width &&
            y2 >= 0 && y2 < dstFrame.Height)
          {
            dstFrame[y2, x2] = srcFrame[y, x];
          }
        }
      }
    }

    public static void DrawWarpedRectangle(Image<Gray, byte> image, Rectangle rect, Matrix<float> W)
    {
      Point leftTop = new Point();
      Point leftBottom = new Point();
      Point rightTop = new Point();
      Point rightBottom = new Point();

      Matrix<float> X = new Matrix<float>(3, 1);
      Matrix<float> Z = new Matrix<float>(3, 1);

      X = CreateVector(rect.X, rect.Y, 1);
      Z = W.Mul(X);
      leftTop.X = (int)Z[0, 0];
      leftTop.Y = (int)Z[1, 0];

      X = CreateVector(rect.X, rect.Y + rect.Height, 1);
      Z = W.Mul(X);
      leftBottom.X = (int)Z[0, 0];
      leftBottom.Y = (int)Z[1, 0];

      X = CreateVector(rect.X + rect.Width, rect.Y, 1);
      Z = W.Mul(X);
      rightTop.X = (int)Z[0, 0];
      rightTop.Y = (int)Z[1, 0];

      X = CreateVector(rect.X + rect.Width, rect.Y + rect.Height, 1);
      Z = W.Mul(X);
      rightBottom.X = (int)Z[0, 0];
      rightBottom.Y = (int)Z[1, 0];

      LineSegment2D a = new LineSegment2D(leftTop, rightTop);
      LineSegment2D b = new LineSegment2D(rightTop, rightBottom);
      LineSegment2D c = new LineSegment2D(rightBottom, leftBottom);
      LineSegment2D d = new LineSegment2D(leftBottom, leftTop);

      image.Draw(a, new Gray(0), 1);
      image.Draw(b, new Gray(0), 1);
      image.Draw(c, new Gray(0), 1);
      image.Draw(d, new Gray(0), 1);
    }


    public static void SetAll(Matrix<float> m, float value)
    {
      for (int i = 0; i < m.Data.GetLength(0); i++)
      {
        for (int j = 0; j < m.Data.GetLength(1); j++)
        {
          m.Data[i, j] = value;
        }
      }
    }

    public static Matrix<float> CreateVector(float x1, float x2, float x3)
    {
      var vector = new Matrix<float>(3, 1);
      vector.Data[0, 0] = x1;
      vector.Data[1, 0] = x2;
      vector.Data[2, 0] = x3;
      return vector;
    }

    public static float[,] GetArray<T>(Image<Gray, T> image)
    {
      var a = new float[image.Width,image.Height];
      for (var i = 0; i < image.Width; i++)
      {
        for (var j = 0; j < image.Height; j++)
        {
          a[i, j] = (float)image[j, i].Intensity;
        }
      }
      return a;
    }

    public static float Interpolate(byte[,] interpolationTable, float x, float y)
    {
      var table = new float[interpolationTable.GetLength(0),interpolationTable.GetLength(1)];
      for (var i = 0; i < interpolationTable.GetLength(0); i++)
      {
        for (var j = 0; j < interpolationTable.GetLength(1); j++)
        {
          table[i, j] = interpolationTable[i, j];
        }
      }
      return Interpolate(table, x, y);
    }

    public static float Interpolate(float[,] interpolationTable, float x, float y)
    {
      // Get the nearest integer pixel coords (xi;yi).
      var x1 = (int)x;
      var y1 = (int)y;
      var x2 = x1 + 1;
      var y2 = y1 + 1;

      bool x2Exist = (x1 < interpolationTable.GetLength(0) - 1);  // Check that pixels to the right  
      bool y2Exist = (y1 < interpolationTable.GetLength(1) - 1); // and to down direction exist.

      var f_x1_y1 = interpolationTable[x1, y1];
      var f_x1_y2 = y2Exist ? interpolationTable[x1, y2] : 0;
      var f_x2_y1 = x2Exist ? interpolationTable[x2, y1] : 0;
      var f_x2_y2 = x2Exist && y2Exist ? interpolationTable[x2, y2] : 0;

      var f_x_y1 = (x2 - x) / (x2 - x1) * f_x1_y1 + (x - x1) / (x2 - x1) * f_x2_y1;
      var f_x_y2 = (x2 - x) / (x2 - x1) * f_x1_y2 + (x - x1) / (x2 - x1) * f_x2_y2;

      var f_x_y = (y2 - y) * (y2 - y1) * f_x_y2 + (y - y1) * (y2 - y1) * f_x_y1;

      return f_x_y;
    }

    public static Matrix<float> SetIdentity(Matrix<float> W,int key)
    {
      for (int i = 0; i < W.Height; i++)
      {
        for (int j = 0; j < W.Width; j++)
        {
          if (i == j)
          {
            W[i, j] = key;
          }
          else
          {
            W[i, j] = 0;
          }
        }
      }
      return W;
    }


  }
}